#include "ship.hpp"
#include "util.hpp"
#include <iostream>

Ship::Ship (std::string shipName, std::string spriteFilename, b2World* gameworld) : PhysicsBody(gameworld)
{
    this->shipName = shipName;
    this->gameWorld = gameWorld;

    //Body Def
    bodyDef.type = b2_dynamicBody;
    bodyDef.position.Set(100,100);   // the body's origin position.
    bodyDef.angle = b2_pi;      // the body's angle in radians.
    bodyDef.linearDamping = 0.02f;
    bodyDef.angularDamping = 0.4f;
    bodyDef.gravityScale = 0.0f; // Set the gravity scale to zero so this body will float
    bodyDef.allowSleep = false;
    //bodyDef.awake = true;
    //bodyDef.fixedRotation = false;
    //bodyDef.userData = this;
    //bodyDef.active = true;

    //shape Def
    b2PolygonShape dynamicBox;
    dynamicBox.SetAsBox(1, 1);

    //fixture def 
    b2FixtureDef fixtureDef;
    fixtureDef.shape = &dynamicBox;
    fixtureDef.density = 5.0f;
    fixtureDef.friction = 0.0f;

    //Fire it up
    this->dynamicBody = this->gameWorld->CreateBody(&bodyDef);
    this->dynamicBody->CreateFixture(&fixtureDef);

    if (!this->shipTexture.loadFromFile(spriteFilename))
    {
       std::cerr<<"Error loading shipTexture"<<std::endl;
    }
    this->shipSprite.setTexture(shipTexture);
    b2Vec2 pos = this->dynamicBody->GetPosition();
    this->shipSprite.setPosition(pos.x, pos.y);
    

    //myWorld->DestroyBody(dynamicBody);
    //dynamicBody = NULL;
}

sf::Sprite Ship::getSprite(){
    return this->shipSprite;
}


b2Vec2 Ship::getPosition(){
    return this->dynamicBody->GetWorldCenter();
}

//void Ship::setPosition(b2Vec2 pos){
//    this->dynamicBody->SetCenterPosition(
//}

float Ship::getAngle(){
    return this->dynamicBody->GetAngle();
}

//void Ship::setRotation(b2Vec2 rot){
//    this->node->setPosition(vector3df(rot.x,rot.y,0.0f));
//}

void Ship::applyForwardThrust(){
    if(!this->shipForwardThrustOn){
        this->shipForwardThrustOn = true;
    }
}

void Ship::cancelForwardThrust(){
    if(this->shipForwardThrustOn){
        this->shipForwardThrustOn = false;
    }
}

void Ship::applyReverseThrust(){
    if(!this->shipReverseThrustOn){
        this->shipReverseThrustOn = true;
    }
}

void Ship::cancelReverseThrust(){
    if(this->shipReverseThrustOn){
        this->shipReverseThrustOn = false;
    }
}

void Ship::applyLeftThrust(){
    if(!this->shipLeftThrustOn){
        this->shipLeftThrustOn = true;
    }
}

void Ship::cancelLeftThrust(){
    if(this->shipLeftThrustOn){
        this->shipLeftThrustOn = false;
    }
}

void Ship::applyRightThrust(){
    if(!this->shipRightThrustOn){
        this->shipRightThrustOn = true;
    }
}

void Ship::cancelRightThrust(){
    if(this->shipRightThrustOn){
        this->shipRightThrustOn = false;
    }
}

void Ship::applyLeftStrafeThrust(){
    if(!this->shipLeftStrafeThrustOn){
        this->shipLeftStrafeThrustOn = true;
    }
}

void Ship::cancelLeftStrafeThrust(){
    if(this->shipLeftStrafeThrustOn){
        this->shipLeftStrafeThrustOn = false;
    }
}

void Ship::applyRightStrafeThrust(){
    if(!this->shipRightStrafeThrustOn){
        this->shipRightStrafeThrustOn = true;
    }
}

void Ship::cancelRightStrafeThrust(){
    if(this->shipRightStrafeThrustOn){
        this->shipRightStrafeThrustOn = false;
    }
}

void Ship::update(){

    //Sync rotation
    float angle = this->dynamicBody->GetAngle();
    this->shipSprite.setRotation(angle);

    //Forward/Reverse thrust
    if(this->shipForwardThrustOn && !this->shipReverseThrustOn){
        b2Vec2 rot = Util::deg2vec(angle);
        rot.Normalize();
        this->dynamicBody->ApplyForce(5*rot, this->dynamicBody->GetWorldCenter());
    }
    else if(this->shipReverseThrustOn && !this->shipForwardThrustOn){
        b2Vec2 rot = Util::deg2vec(angle);
        rot.Normalize();
        this->dynamicBody->ApplyForce(-3*rot, this->dynamicBody->GetWorldCenter());
    }

    //Strafe thrust
    if(this->shipLeftStrafeThrustOn && !this->shipRightStrafeThrustOn){
        b2Vec2 rot = Util::deg2vec(angle+90);
        rot.Normalize();
        this->dynamicBody->ApplyForce(2*rot, this->dynamicBody->GetWorldCenter());
    }
    else if(this->shipRightStrafeThrustOn && !this->shipLeftStrafeThrustOn){
        b2Vec2 rot = Util::deg2vec(angle-90);
        rot.Normalize();
        this->dynamicBody->ApplyForce(2*rot, this->dynamicBody->GetWorldCenter());
    }

    //Sync position
    b2Vec2 pos = this->dynamicBody->GetPosition();
    this->shipSprite.setPosition(pos.x, pos.y);
    
    //Left/Right thrust
    if(this->shipLeftThrustOn && !this->shipRightThrustOn){
        this->dynamicBody->ApplyTorque(10.0f);
    }
    else if(this->shipRightThrustOn && !this->shipLeftThrustOn){
        this->dynamicBody->ApplyTorque(-10.0f);
    }
    std::cout << pos.x << ", " << pos.y << std::endl;


}

